#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/filter.h>

#include <iostream>
using namespace cv;
using namespace pcl;
using namespace std;
int main(int argc, char** argv)
{
    // read a image and a pcd
    Mat image_origin = Mat::zeros(1024,1280,CV_8UC3);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_origin(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_withoutNAN(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::io::loadPCDFile<pcl::PointXYZI> ("../1.pcd", *cloud_origin);
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*cloud_origin, *cloud_withoutNAN, indices);

    std::vector<cv::Point3f> pts_3d;
    for (size_t i = 0; i < cloud_withoutNAN->size(); ++i)
    {
        pcl::PointXYZI point_3d = cloud_withoutNAN->points[i];
        if (point_3d.x > 2 && point_3d.x < 3 && point_3d.y > -10 && point_3d.y < 10)
        {
            pts_3d.emplace_back(cv::Point3f(point_3d.x, point_3d.y, point_3d.z));
        }
    }

    // using iterator

    // read calibration parameter
    cv::Mat camera_matrix = (cv::Mat_<double>(3, 3) << 1.3859739625395162e+03, 0., 9.3622464596653492e+02, 0.,
            1.3815353250336800e+03, 4.9459467170828475e+02, 0., 0., 1.);
    cv::Mat distortion_coeff = (cv::Mat_<double>(1, 5) << 7.0444095385902794e-02, -1.8010798300183417e-01,
            -7.7001990711544465e-03, -2.2524968464184810e-03,
            1.4838608095798808e-01);
    cv::Mat r_vec = (cv::Mat_<double>(3, 3) << -7.1907391850483116e-03, 1.0494953004635377e-02,
            9.9991907134097757e-01,-9.9997142335005851e-01, 2.2580773589691017e-03,
            -7.2148159989590677e-03,-2.3336137706425064e-03, -9.9994237686382270e-01,
            1.0478415848689249e-02);
    cv::Mat t_vec = (cv::Mat_<double>(3, 1) << 1.0984281510814174e-01,-1.8261670813403203e-02,1.7323651488230618e-01);

    // project 3d-points into image view
    std::vector<cv::Point2f> pts_2d;
    cv::projectPoints(pts_3d, r_vec, t_vec, camera_matrix, distortion_coeff, pts_2d);

    cv::Mat image_project = image_origin.clone();
    int image_rows = image_origin.rows;
    int image_cols = image_origin.cols;
    int count=0;
    for (size_t i = 0; i < pts_2d.size(); ++i)
    {
        cv::Point2f point_2d = pts_2d[i];
//        if (point_2d.x < 0 || point_2d.x > image_cols || point_2d.y < 0 || point_2d.y > image_rows)
//        {
//            continue;
//        }
//        else
//        {
//            image_project.at<cv::Vec3b>(point_2d.y, point_2d.x)[0] = 0;
//            image_project.at<cv::Vec3b>(point_2d.y, point_2d.x)[1] = 0;
//            image_project.at<cv::Vec3b>(point_2d.y, point_2d.x)[2] = 255;
//            count += 1;
//        }

        if (point_2d.x > 0 && point_2d.x < image_cols && point_2d.y > 0 && point_2d.y < image_rows)
        {
            image_project.at<cv::Vec3b>(point_2d.y, point_2d.x)[0] = 0;
            image_project.at<cv::Vec3b>(point_2d.y, point_2d.x)[1] = 0;
            image_project.at<cv::Vec3b>(point_2d.y, point_2d.x)[2] = 255;
            count += 1;
        }
        else
        {
            continue;
        }
    }
    cout<<count;
   // cv::imshow("origin image", image_origin);
    cv::imshow("project image", image_project);
   // cv::imwrite("/media/data/temp/image_project.jpg", image_project);
waitKey(-1);

    return 0;

}